From cefe563cf482b5726660e15b986a9eedc88e1fdf Mon Sep 17 00:00:00 2001 From: parkrrrr Date: Thu, 31 Jul 2003 14:26:43 +0000 Subject: [PATCH] lots of math optimizations --- gpsbabel/arcdist.c | 155 +++++++++++++++++++++++++++++++-------------- 1 file changed, 106 insertions(+), 49 deletions(-) diff --git a/gpsbabel/arcdist.c b/gpsbabel/arcdist.c index 66aef4f21..dc0cc8869 100644 --- a/gpsbabel/arcdist.c +++ b/gpsbabel/arcdist.c @@ -53,53 +53,89 @@ static double gcdist( double lat1, double lon1, double lat2, double lon2 ) { return acos(sin(lat1)*sin(lat2)+cos(lat1)*cos(lat2)*cos(lon1-lon2)); } + +static void crossproduct( double x1, double y1, double z1, + double x2, double y2, double z2, + double *xa, double *ya, double *za ) { + *xa = y1*z2-y2*z1; + *ya = z1*x2-z2*x1; + *za = x1*y2-y1*x2; +} + +static double dotproduct( double x1, double y1, double z1, + double x2, double y2, double z2 ) { + return (x1*x2+y1*y2+z1*z2); +} static double linedist(double lat1, double lon1, double lat2, double lon2, double lat3, double lon3 ) { - double lat4, lon4; - double x1,y1,z1; - double x2,y2,z2; + static double _lat1 = -9999; + static double _lat2 = -9999; + static double _lon1 = -9999; + static double _lon2 = -9999; + + static double x1,y1,z1; + static double x2,y2,z2; + static double xa,ya,za,la; + double x3,y3,z3; - - double xa,ya,za,la; - double xp,yp,zp,lp; + + double xa1,ya1,za1; + double xa2,ya2,za2; + + double d1, d2; + double c1, c2; double dot; - double d12, d1p, dp2; - - double dist; - + int newpoints; + /* degrees to radians */ lat1 *= M_PI/180.0; lon1 *= M_PI/180.0; lat2 *= M_PI/180.0; lon2 *= M_PI/180.0; lat3 *= M_PI/180.0; lon3 *= M_PI/180.0; + newpoints = 1; + if ( lat1 == _lat1 && lat2 == _lat2 && lon1 == _lon1 && lon2 == _lon2) { + newpoints = 0; + } + else { + _lat1 = lat1; + _lat2 = lat2; + _lon1 = lon1; + _lon2 = lon2; + } + /* polar to ECEF rectangular */ - x1 = cos(lon1)*cos(lat1); y1 = sin(lat1); z1 = sin(lon1)*cos(lat1); - x2 = cos(lon2)*cos(lat2); y2 = sin(lat2); z2 = sin(lon2)*cos(lat2); + if ( newpoints ) { + x1 = cos(lon1)*cos(lat1); y1 = sin(lat1); z1 = sin(lon1)*cos(lat1); + x2 = cos(lon2)*cos(lat2); y2 = sin(lat2); z2 = sin(lon2)*cos(lat2); + } x3 = cos(lon3)*cos(lat3); y3 = sin(lat3); z3 = sin(lon3)*cos(lat3); + if ( newpoints ) { /* 'a' is the axis; the line that passes through the center of the earth * and is perpendicular to the great circle through point 1 and point 2 * It is computed by taking the cross product of the '1' and '2' vectors.*/ - xa = y1*z2-y2*z1; - ya = z1*x2-z2*x1; - za = x1*y2-y1*x2; - la = sqrt(xa*xa+ya*ya+za*za); + crossproduct( x1, y1, z1, x2, y2, z2, &xa, &ya, &za ); + la = sqrt(xa*xa+ya*ya+za*za); + if ( la ) { + xa /= la; + ya /= la; + za /= la; + } + } if ( la ) { - xa /= la; - ya /= la; - za /= la; /* dot is the component of the length of '3' that is along the axis. * What's left is a non-normalized vector that lies in the plane of * 1 and 2. */ - dot = x3*xa+y3*ya+z3*za; + + dot = dotproduct(x3,y3,z3,xa,ya,za); xp = x3-dot*xa; yp = y3-dot*ya; @@ -111,50 +147,71 @@ static double linedist(double lat1, double lon1, xp /= lp; yp /= lp; zp /= lp; - - /* convert the point 'p' (the closest point to '3' on the great circle - * that passest through '1' and '2') back to lat/lon */ - lat4 = asin( yp ); - lon4 = atan2( zp/cos(lat4), xp/cos(lat4)); - - /* compute a few distances */ - d12 = gcdist(lat1,lon1,lat2,lon2); - d1p = gcdist(lat1,lon1,lat4,lon4); - dp2 = gcdist(lat4,lon4,lat2,lon2); + + crossproduct(x1,y1,z1,xp,yp,zp,&xa1,&ya1,&za1); + d1 = dotproduct( xa1, ya1, za1, xa, ya, za ); + + crossproduct(xp,yp,zp,x2,y2,z2,&xa2,&ya2,&za2); + d2 = dotproduct( xa2, ya2, za2, xa, ya, za ); + + if ( d1 >= 0 && d2 >= 0 ) { + /* rather than call gcdist and all its sines and cosines and + * worse, we can get the angle directly. It's the arctangent + * of the length of the component of vector 3 along the axis + * divided by the length of the component of vector 3 in the + * plane. We already have both of those numbers. + * + * atan2 would be overkill because lp and fabs(dot) are both + * known to be positive. */ + + return atan( fabs(dot)/lp ); + } - if ( d12 >= d1p && d12 >= dp2 ) { - /* if 'p' is closer to both 1 and 2 than 1 is to 2, p is between. */ - dist = gcdist(lat3,lon3,lat4,lon4); + /* otherwise, get the distance from the closest endpoint */ + c1 = dotproduct( x1,y1,z1,xp,yp,zp ); + c2 = dotproduct( x2,y2,z2,xp,yp,zp ); + d1 = labs(d1); + d2 = labs(d2); + + /* This is a hack. d$n$ is proportional to the sine of the angle + * between point $n$ and point p. That preserves orderedness up + * to an angle of 90 degrees. c$n$ is proportional to the cosine + * of the same angle; if the angle is over 90 degrees, c$n$ is + * negative. In that case, we flop the sine across the y=1 axis + * so that the resulting value increases as the angle increases. + * + * This only works because all of the points are on a unit sphere. */ + + if ( c1 < 0 ) { + d1 = 2 - d1; + } + if ( c2 < 0 ) { + d2 = 2 - d2; + } + + if ( labs(d1) < labs(d2)) { + return gcdist(lat1,lon1,lat3,lon3); } else { - /* otherwise, get the distance from the closest endpoint */ - if ( d1p < dp2 ) { - dist = gcdist(lat1,lon1,lat3,lon3); - } - else { - dist = gcdist(lat2,lon2,lat3,lon3); - } + return gcdist(lat2,lon2,lat3,lon3); } } else { /* lp is 0 when 3 is 90 degrees from the great circle */ - dist = M_PI/2; + return M_PI/2; } } else { /* la is 0 when 1 and 2 are either the same point or 180 degrees apart */ - d12 = gcdist(lat1,lon1,lat2,lon2); - if ( d12 < 1 ) { - /* less than a radian : same point */ - dist = gcdist(lat1,lon1,lat3,lon3); + dot = dotproduct(x1,y1,z1,x2,y2,z2); + if ( dot >= 0 ) { + return gcdist(lat1,lon1,lat3,lon3); } else { - /* more than a radian : 180 degrees apart; there's a great circle that - * goes through all 3 points, so the distance is 0 */ - dist = 0; + return 0; } } - return dist; + return 0; } #define BADVAL 999999 -- 2.30.2